﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.IO;
using System.Net.Sockets;
using System.Threading;
using System.Drawing.Drawing2D;
using System.Globalization;

// CCR & DSS Namespaces
using Microsoft.Ccr.Core;
using Microsoft.Ccr;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Core.DsspHttp;

namespace JaguarControl
{
    /// <summary>
    ///  THE LASER SENSOR PROVIDES A 240° RANGE WITH A RESOLUTION OF 1.0572° GIVING A 227 VALUES READING, RANGING FROM 20mm TO 
    ///  4000mm. 
    ///  FOR BETTER OPERATIONS AND TAKING INTO ACCOUNT THE LASER PHYSICAL LOCATION INSIDE OF THE ROBOT, THE RANGE WAS 
    ///  LIMITED TO [20mm,3000mm] IN DISTANCE AND TO [111°; 35.55°,146.55°] IN ANGLE, GIVING USED ARRAY VALUES FROM [62,167] IN THE 
    ///  ORIGINAL 227 ARRAY.
    ///  Obstacle detection is at distances over 20mm, and
    ///  Directly in front of the left wheel must be 65mm, and
    ///  Directly in front of the right wheel must be 130mm.
    /// </summary>
    public partial class MainForm : Form
    {
        #region Variables
        private TcpClient clientSocketLaser = null;
        private Thread threadClientLaser = null;
        private NetworkStream cmdStreamLaser = null;
        private BinaryReader readerLaser = null;
        private BinaryWriter writerLaser = null;
        private bool receivingLaser = false;
        /*
        private int monitorCntLaser = 0;
        private int errorChkCntLaser = 0;
        private bool firstSetupCommLaser = true;
        */
        //private bool connectedLaser = false;
        private delegate void updateLaserSensorDataInfo(byte[] data);

        private const string SCANCOMMAND = "GD0045072503";
        //below variables are based on above scan command, 
        private const int DISDATALEN = 227; // based on scan command, it will be 680 distance data:   1.0572687° Resolution
        private const int DATALEN = 681;
        private const int DATALINE = 11;    // 11 data lines(each line = 64 byte data + sum + LF, 
        public int[] laserData = new int[DISDATALEN];
        private long[] disData = new long[DISDATALEN];
        private byte[] dataArray = new Byte[DATALEN];
        private long[] disDataLast = new long[DISDATALEN];
        //this program will get data from -120 to +120. (G04572401) 724-44 = 680;
        private const double startAng = -31.5 / 180 * Math.PI; //27.0 / 180 * Math.PI;
        private const double stepAng = 0.3529 / 180 * Math.PI * 3;//2.1 / 180 * Math.PI;    cluster = 03
        private const double ratio = 50;
        private const int START_X = 0;
        private const int START_Y = 120;
        private const int WIDTH_SUB = 40;

        #endregion

        private void startCommLaser()
        {
            int remotePort = 10002;

            String IPAddr = jaguarSetting.LaserRangeIP;
            try
            {
                clientSocketLaser = new TcpClient();
                IAsyncResult results = clientSocketLaser.BeginConnect(IPAddr, remotePort, null, null);
                bool success = results.AsyncWaitHandle.WaitOne(500, true);

                if (!success)
                {
                    clientSocketLaser.Close();
                    clientSocketLaser = null;
                    receivingLaser = false;
                }
                else
                {
                    receivingLaser = true;
                    threadClientLaser = new Thread(new ThreadStart(HandleClientLaser));
                    threadClientLaser.CurrentCulture = new CultureInfo("en-US");
                    threadClientLaser.Start();
                }
            }
            catch
            {

            }
            
            if (clientSocketLaser != null)
            {
                sendCommandLaser("BM");
            }

            if (clientSocketLaser != null)
            {
                sendCommandLaser(SCANCOMMAND);
            }

        }

        private void stopCommunicationLaser()
        {
            if (clientSocketLaser != null)
                clientSocketLaser.Close();



            receivingLaser = false;
            if (writerLaser != null)
                writerLaser.Close();
            if (readerLaser != null)
                readerLaser.Close();
            if (cmdStreamLaser != null)
                cmdStreamLaser.Close();
            if (threadClientLaser != null)
                threadClientLaser.Abort();

        }

        private void HandleClientLaser()
        {

            //here process all receive data from client
            try
            {
                cmdStreamLaser = clientSocketLaser.GetStream();
                readerLaser = new BinaryReader(cmdStreamLaser);
                writerLaser = new BinaryWriter(cmdStreamLaser);

                bool keepSearch = true;
                byte[] decodeBuff = new byte[2048];  //maxlength
                int lastPos = 0;
                int processLen = 0;
                int endPos = 0;

                while (receivingLaser)
                {
                    try
                    {

                        if (!cmdStreamLaser.DataAvailable)      //there is no data
                        {
                            //need to sleep for other thread
                            Thread.Sleep(50);
                            //For re-scanning and re-synchronizing when thread has dropped laser readings.
                            sendCommandLaser("BM");
                            sendCommandLaser(SCANCOMMAND);

                        }
                        else
                        {
                            // Reads NetworkStream into a byte buffer.

                            //errorChkCntLaser = 0;
                            byte[] bytes = new byte[2048];
                            // Read can return anything from 0 to numBytesToRead. 
                            // This method blocks until at least one byte is read.
                            processLen = readerLaser.Read(bytes, 0, bytes.Length);
                            Array.Copy(bytes, 0, decodeBuff, lastPos, processLen);
                            processLen += lastPos;
                            keepSearch = true;
                            endPos = 0;
                            lastPos = processLen;

                            while (keepSearch)
                            {
                                endPos = 0;
                                if (processLen >= 2)
                                {
                                    for (int i = 0; i < processLen - 1; i++)
                                    {
                                        if ((decodeBuff[i] == 10) && (decodeBuff[i + 1] == 10))
                                        {
                                            endPos = i;
                                            break;
                                        }
                                    }
                                }


                                if (endPos > 0)
                                {
                                    // already find the end, cut the string to decode command

                                    //string recCommand = Encoding.Unicode.GetString(decodeBuff, 0, endPos);
                                    byte[] recCommand = new byte[endPos + 2];
                                    Array.Copy(decodeBuff, 0, recCommand, 0, endPos + 2);
                                    //process receive data here
                                    processDataLaser(recCommand);

                                    if ((endPos + 2) >= processLen)
                                    {
                                        //stop search
                                        keepSearch = false;
                                        lastPos = 0;
                                    }
                                    else
                                    {
                                        // trim this package from decode buff
                                        int len = processLen - endPos - 2;
                                        byte[] temp = new byte[len];

                                        Array.Copy(decodeBuff, endPos + 2, temp, 0, len);
                                        Array.Copy(temp, 0, decodeBuff, 0, len);
                                        lastPos = len;
                                        processLen = len;
                                    }
                                }
                                else
                                {
                                    keepSearch = false;
                                    //lastPos = 0;
                                    processLen = 0;
                                }
                            }
                        }
                    }
                    catch
                    {
                    }
                    finally
                    {
                    }

                }
                readerLaser.Close();
                cmdStreamLaser.Close();
            }
            catch
            {
            }
            receivingLaser = false;


            clientSocketLaser.Close();

        }

        private void processDataLaser(byte[] data)
        {

            Invoke(new updateLaserSensorDataInfo(updateSensor), data);
        }

        private void updateSensor(byte[] msg)
        {
            string rcvData = Encoding.ASCII.GetString(msg);


            StringReader dataRead = new StringReader(rcvData);
            string ackData = "";
            try
            {
                ackData = dataRead.ReadLine();
            }
            catch
            {
            }
            string status = "";
            try
            {
                status = dataRead.ReadLine().Remove(2, 1);
            }
            catch
            {
            }
            string timeStamp = "";
            try
            {
                timeStamp = dataRead.ReadLine().Remove(4, 1);
            }
            catch
            {
            }
            long timeData = 0;
            byte[] tempData = null;
            if (timeStamp.Length == 4)
            {
                tempData = System.Text.Encoding.ASCII.GetBytes(timeStamp);

                timeData = (((long)(tempData[0] - 0x30)) << 18) + (((long)(tempData[1] - 0x30)) << 12)
                + (((long)(tempData[2] - 0x30)) << 6) + ((long)(tempData[3] - 0x30));
            }

            string temp = "";
            int lineCnt = 0;
            if ((status == "00") && (msg.Length == 727))
            {
                //suppose all the data is right
                while (lineCnt < DATALINE - 1)
                {
                    temp = dataRead.ReadLine();       //remove sum and LF, won't check sum here
                    temp = temp.Remove(64, 1);
                    tempData = System.Text.Encoding.ASCII.GetBytes(temp);
                    Array.Copy(tempData, 0, dataArray, lineCnt * 64, tempData.Length);
                    lineCnt++;
                }
                if (lineCnt == DATALINE - 1)
                {
                    temp = dataRead.ReadLine();
                    temp = temp.Remove(41, 1);       //the last line is 41 byte data + sum + LF + LF
                    tempData = System.Text.Encoding.ASCII.GetBytes(temp);
                    Array.Copy(tempData, 0, dataArray, lineCnt * 64, tempData.Length);
                }

                //transfer data to distance sta
                for (int i = 0; i < DISDATALEN; i++)
                {
                    disData[i] = ((long)(dataArray[3 * i] - 0x30) << 12) + (((long)(dataArray[3 * i + 1] - 0x30)) << 6) + (long)(dataArray[3 * i + 2] - 0x30);
                }

                drawSensor(DISDATALEN);
                //Raw data to clean data
                Raw2Clean();
            }

        }

        private void sendCommandLaser(string s)
        {
            try
            {
                s = s + new string((char)13, 1);

                ASCIIEncoding ascCode = new ASCIIEncoding();
                writerLaser.Flush();
                byte[] bytes = ascCode.GetBytes(s);
                writerLaser.Write(bytes, 0, bytes.Length);

            }
            catch
            {
            }
        }

        private void Raw2Clean()
        {
            for (int i = 0; i < DISDATALEN; ++i)
            {
                //initialize
                laserData[i] = 0;
                //set value
                if (disData[i] <= 3000 && disData[i] >= 20)
                    laserData[i] = (int)disData[i];
                else
                    laserData[i] = (int)3000;
                if (i < 62 || i > 167)
                    laserData[i] = 0;
                //if (i == 114 || i == 115 || i == 80 || i == 97 || i == 133 || i == 150)
                //    laserData[i] = 0;
            }

        }

        #region Displaying
        private void drawLaserBackground()
        {
            Bitmap bmp = new Bitmap(pictureBoxLASER.Width, pictureBoxLASER.Height);
            Graphics g = Graphics.FromImage(bmp);

            int width = bmp.Width - 50;
            int height = bmp.Height;

            g.Clear(Color.Lavender);
            g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;

            int centerX = bmp.Width / 2;
            int centerY = bmp.Height - 35;
            //g.DrawLine(Pens.Blue, 0, bmp.Height- 120, bmp .Width , bmp.Height -120 );
            //g.DrawLine(Pens.Blue, bmp.Width /2, 0, bmp.Width/2 , bmp.Height- 120);

            for (int i = 0; i < 6; ++i)
            {
                Rectangle RECT = new Rectangle(centerX - 25 * (i + 1), centerY - 25 * (i + 1), 25 * (i + 1) * 2, 25 * (i + 1) * 2);

                g.DrawArc(Pens.Black, RECT, 180, 180);

                //g.DrawArc(Pens.Green, centerX - 25 * (i + 1), centerY - 25 * (i + 1), 25 * (i + 1) * 2, 25 * (i + 1) * 2, 150, 240);
            }

            Font drawFont = new Font("Arial", 8);
            SolidBrush drawBrush = new SolidBrush(Color.Black);

            //Angle Lines
            for (int i = 0; i < 19; ++i)
            {
                float cosValue = (float)Math.Cos(10.0 * i / 180 * Math.PI);
                float sinValue = (float)Math.Sin(10.0 * i / 180 * Math.PI);
                float x = (float)(centerX + width / 1.7 * cosValue);
                float y = (float)(centerY - width / 1.7 * sinValue);

                if ((i < 22) || (i > 32))
                {
                    g.DrawLine(Pens.Black, centerX, centerY, x, y);
                }

            }

            RectangleF drawRect = new RectangleF(centerX - 35, centerY, 50, 50);
            g.DrawString("0.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 58, centerY, 50, 50);
            g.DrawString("1.0", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 82, centerY, 50, 50);
            g.DrawString("1.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 105, centerY, 50, 50);
            g.DrawString("2.0", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 130, centerY, 50, 50);
            g.DrawString("2.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 153, centerY, 50, 50);
            g.DrawString("3.0", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 183, centerY, 50, 50);
            //g.DrawString("3.5", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 208, centerY, 50, 50);
            //g.DrawString("4.0", drawFont, drawBrush, drawRect);

            drawRect = new RectangleF(centerX + 15, centerY, 50, 50);
            g.DrawString("0.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 38, centerY, 50, 50);
            g.DrawString("1.0", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 63, centerY, 50, 50);
            g.DrawString("1.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 88, centerY, 50, 50);
            g.DrawString("2.0", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 113, centerY, 50, 50);
            g.DrawString("2.5", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 135, centerY, 50, 50);
            g.DrawString("3.0", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 165, centerY, 50, 50);
            //g.DrawString("3.5", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 190, centerY, 50, 50);
            //g.DrawString("4.0", drawFont, drawBrush, drawRect);


            drawRect = new RectangleF(centerX-6, centerY - 162, 50, 50);
            g.DrawString("0", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 27, centerY - 210, 50, 50);
            //g.DrawString("-10", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 60, centerY - 202, 50, 50);
            //g.DrawString("-20", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 130, centerY - 85, 50, 50);
            g.DrawString("60", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 125, centerY - 165, 50, 50);
            //g.DrawString("-40", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 153, centerY - 140, 50, 50);
            //g.DrawString("-50", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX + 72, centerY - 142, 50, 50);
            g.DrawString("30", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 190, centerY - 78, 50, 50);
            //g.DrawString("-70", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 200, centerY - 43, 50, 50);
            //g.DrawString("-80", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 203, centerY - 8, 50, 50);
            //g.DrawString("-90", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX + 198, centerY + 28, 50, 50);

            //drawRect = new RectangleF(centerX - 42, centerY - 210, 50, 50);
            //g.DrawString("10", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 75, centerY - 202, 50, 50);
            //g.DrawString("20", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 90, centerY - 140, 50, 50);
            g.DrawString("30", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 140, centerY - 165, 50, 50);
            //g.DrawString("40", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 165, centerY - 140, 50, 50);
            //g.DrawString("50", drawFont, drawBrush, drawRect);
            drawRect = new RectangleF(centerX - 147, centerY - 81, 50, 50);
            g.DrawString("60", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 203, centerY - 78, 50, 50);
            //g.DrawString("70", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 213, centerY - 43, 50, 50);
            //g.DrawString("80", drawFont, drawBrush, drawRect);
            //drawRect = new RectangleF(centerX - 215, centerY - 8, 50, 50);
            //g.DrawString("90", drawFont, drawBrush, drawRect);

            pictureBoxLASER.BackgroundImage = bmp;
        }

        private void drawSensor(int len)
        {
            if (!laserfrontview)   //DRAW THE LASER FROM TOP VIEW
            {
                Bitmap sensorImge = new Bitmap(pictureBoxLASER.Width, pictureBoxLASER.Height);
                Graphics g = Graphics.FromImage(sensorImge);
                int width = sensorImge.Width - WIDTH_SUB;
                int height = sensorImge.Height;

                int centerX = sensorImge.Width / 2 - START_X, centerY = sensorImge.Height - START_Y + 85;

                g.Clear(Color.Transparent);

                g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.HighQuality;

                SolidBrush dataBrush = null;
                PointF[] curvePoints = new PointF[len + 1];

                // Define fill mode.
                FillMode newFillMode = FillMode.Winding;

                curvePoints[len] = new PointF((float)centerX, (float)centerY);
                
                dataBrush = new SolidBrush(Color.Lime);
                for (int i = 0; i < DISDATALEN; ++i)
                {
                    //HERE WE MUST FIND THE INDEXES FOR i CORRESPONDING TO AN ANGLE RANGE FROM -65,65 AT LEAST
                    float Y = (float)(centerY - ratio * (double)laserData[i] / 1000 * Math.Sin(startAng + stepAng * i));
                    float X = (float)(centerX + ratio * (double)laserData[i] / 1000 * Math.Cos(startAng + stepAng * i));
                    curvePoints[i] = new PointF(X, Y);
                }
                // Fill polygon to screen.
                g.FillPolygon(dataBrush, curvePoints, newFillMode);

               pictureBoxLASER.Image = sensorImge;
            }
            else //DRAW THE LASER FROM FRONT VIEW
            {
                Bitmap bmp = new Bitmap(pictureBoxLASER.Width, pictureBoxLASER.Height);
                Graphics g = Graphics.FromImage(bmp);
                int width = bmp.Width;

                g.Clear(Color.Lavender);
                int half = bmp.Height / 2;

                for (int x = 62; x < 168; x++)
                {
                    double i = (x - 62)*2.83;
                    Color col = LinearColor(Color.Red, Color.Lavender, 0, 3200, laserData[x]);
                    g.DrawLine(new Pen(col), width - (int)i, half - (half / 24) * 3000 / laserData[x], width - (int)i, half + (half / 24) * 3000 / laserData[x]);
                    g.DrawLine(new Pen(col), width - (int)i + 1, half - (half / 24) * 3000 / laserData[x], width - (int)i + 1, half + (half / 24) * 3000 / laserData[x]);
                    g.DrawLine(new Pen(col), width - (int)i + 2, half - (half / 24) * 3000 / laserData[x], width - (int)i + 2, half + (half / 24) * 3000 / laserData[x]);
                }

                pictureBoxLASER.Image = bmp;    
            }
        }

        private Color LinearColor(Color nearColor, Color farColor, int nearLimit, int farLimit, int value)
        {
            if (value <= nearLimit)
            {
                return nearColor;
            }
            else if (value >= farLimit)
            {
                return farColor;
            }

            int span = farLimit - nearLimit;
            int pos = value - nearLimit;

            int r = (nearColor.R * (span - pos) + farColor.R * pos) / span;
            int g = (nearColor.G * (span - pos) + farColor.G * pos) / span;
            int b = (nearColor.B * (span - pos) + farColor.B * pos) / span;

            return Color.FromArgb(r, g, b);
        }
        #endregion

        #region Mapping
        private void drawMapBackground()
        {
            Bitmap bmp = new Bitmap(picMap.Width, picMap.Height);
            Graphics g = Graphics.FromImage(bmp);

            g.Clear(Color.White);
            picMap.Image = bmp;
        }

        //    /// <summary>
        //    /// Modes (methods) for producing a map 
        //    /// </summary>
        //    [DataContract]
        //    public enum MapMode
        //    {
        //        Overwrite,      // Just write new data (free or occupied) into the map
        //        Counting       // Count the number of times a cell is seen free (or occ)
        //        //BayesRule       // Use Bayes Rule to update cell values
        //    }

        //    // Values for the occupancy grid
        //    // Because the values are stored as a single byte,
        //    // they range from 0 to 255. However, there are
        //    // thresholds for occupied/vacant cells because
        //    // it is not necessary for the probabilities to
        //    // reach 255 or 0 respectively.
        //    // The range is arbitrarily divided into three sections:
        //    // Occupied =   0 ->  64
        //    // Unknown  =  65 -> 191
        //    // Vacant   = 192 -> 255
        //    // This conforms to the convention that is commonly used for
        //    // occupancy grids in the literature where:
        //    // Black (Occupied) represents obstacles, 
        //    // Grey  (Unknown) is unknown, and
        //    // White (Free) is free (clear or empty) space
        //    // For convenience, two other values (and aliases) are also
        //    // defined, although they might be a little confusing:
        //    // OBSTACLE = MAP_UNKNOWN_LOW is the UPPER bound for an
        //    // obstacle, or the LOWER bound for unknown space
        //    // MAP_VACANT = MAP_UNKNOWN_HIGH is the LOWER bound for free
        //    // (vacant) space, or the UPPER bound for unknown space
        //    public enum MapValues
        //    {
        //        OCCUPIED = 0,
        //        OBSTACLE = 64,
        //        // Value for 50% probabilty of occupied, i.e. unknown space
        //        UNKNOWN = 128,
        //        VACANT = 192,
        //        FREE = 255,

        //        // Range for Unknown (synonyms for OBSTACLE and VACANT)
        //        UNKNOWN_LOW = 64,
        //        UNKNOWN_HIGH = 192
        //    }

        //    /// <summary>
        //    /// Mapper - Class for performing mapping
        //    /// </summary>
        //    class Mapper
        //    {
        //        /// <summary>
        //        /// Drawing mode for the map : Default value is Overwrite, which sets either 0 or 255
        //        /// </summary>
        //        public MapMode mode = MapMode.Overwrite;

        //        // Number of range points in each scan
        //        private int _num_ranges;

        //        // Maximum accepted range
        //        private double _range_max;

        //        // Start angle for scans
        //        private double _range_start;

        //        // Angular resolution of scans
        //        private double _range_step;

        //        // Grid resolution and dimensions
        //        private double _grid_res;
        //        private int _grid_sx, _grid_sy, _grid_size;


        //        // Conversion factor for range scans
        //        // Scans are in millimeters, but we work in meters
        //        private const int _range_factor = 1000;

        //        // The actual map itself is a 2D array of bytes        
        //        private byte[,] _mapData = null;

        //        public byte[,] GetMapData()
        //        {
        //            return _mapData;
        //        }

        //        // The local map that is used during updates with Bayes Rule
        //        private byte[,] _localMapData = null;

        //        // This was an attempt to encapsulate the map image,
        //        // but there are problems with threading and the GDI
        //        private static Bitmap _mapImage = null;
        //        //private static Graphics g = null;

        //        public Bitmap GetMapImage()
        //        {
        //            return _mapImage;
        //        }

        //        /// <summary>
        //        /// Init - Set parameters and do the necessary allocations
        //        /// </summary>
        //        /// <remarks>
        //        /// NOTE: This method must be called before any others!
        //        /// </remarks>
        //        /// <param name="num_ranges">Number of scans</param>
        //        /// <param name="range_max">Max range</param>
        //        /// <param name="range_start">Start angle for scan</param>
        //        /// <param name="range_step">Increment between scans</param>
        //        /// <param name="width">Width of map in meters</param>
        //        /// <param name="height">Height of map in meters</param>
        //        /// <param name="grid_res">Resolution of map in meters (grid size)</param>
        //        /// <returns></returns>
        //        public Bitmap Init(int num_ranges, double range_max,
        //                   double range_start, double range_step,
        //                   double width, double height, double grid_res)
        //        {
        //            // Record range settings
        //            _num_ranges = num_ranges;
        //            _range_max = range_max;
        //            _range_start = range_start;
        //            _range_step = range_step;

        //            // Allocate space for grid
        //            _grid_res = grid_res;
        //            _grid_sx = (int)Math.Ceiling(width / _grid_res);
        //            // Ensure that the X axis has a multiple of 4 pixels,
        //            // i.e. it will be longword aligned when the bitmap
        //            // image is created
        //            if ((_grid_sx % 4) != 0)
        //            {
        //                _grid_sx += 4 - (_grid_sx % 4);
        //            }
        //            _grid_sy = (int)Math.Ceiling(height / _grid_res);
        //            _grid_size = _grid_sx * _grid_sy;
        //            // Allocate memory for the maps
        //            _mapData = new byte[_grid_sx, _grid_sy];
        //            _localMapData = new byte[_grid_sx, _grid_sy];

        //            Bitmap _mapImage = new Bitmap(_grid_sx, _grid_sy, PixelFormat.Format24bppRgb);
        //            //g = Graphics.FromImage(_mapImage);
        //            //g.Clear(Color.LightGray);
        //            // Clear() could probably be called as a convenience

        //            return _mapImage;
        //        }


        //        /// <summary>
        //        /// Clear() - Erase the map
        //        /// </summary>
        //        public void Clear()
        //        {
        //            int i, j;
        //            for (j = 0; j < _grid_sy; j++)
        //                for (i = 0; i < _grid_sx; i++)
        //                    _mapData[i, j] = (byte)MapValues.UNKNOWN;

        //            return;
        //        }

        //        //private void ClearLocal()
        //        //{
        //        //    int i, j;
        //        //    for (j = 0; j < _grid_sy; j++)
        //        //        for (i = 0; i < _grid_sx; i++)
        //        //            _localMapData[i, j] = (byte)MapValues.UNKNOWN;

        //        //    return;
        //        //}

        //        // Convert from real-world coord to map coord


        //        private int MapGridX(double x)
        //        {
        //            return (int)Math.Floor(x / _grid_res) + _grid_sx / 2;
        //        }

        //        private int MapGridY(double y)
        //        {
        //            return (int)Math.Floor(y / _grid_res) + _grid_sy / 2;
        //        }

        //        // Check that map coords are inside the array
        //        private bool MapGridValid(int x, int y)
        //        {
        //            return ((x >= 0) && (x < _grid_sx) &&
        //                    (y >= 0) && (y < _grid_sy));
        //        }


        //        /// <summary>
        //        /// Add - Adds a new laser range scan to the map
        //        /// </summary>
        //        /// <param name="x">Robot position</param>
        //        /// <param name="y">Robot position</param>
        //        /// <param name="theta">Robot orientation</param>
        //        /// <param name="num_ranges">Number of range values (redundant)</param>
        //        /// <param name="ranges">Array of range values in millimeters</param>
        //        public void Add(float x, float y, float theta, int num_ranges, int[] ranges)
        //        {
        //            int i, j;
        //            double r, dx, dy, dr;
        //            double ax, ay, bx, by, px, py;
        //            int step_count;
        //            double step_size;
        //            int nx, ny, occ;
        //            bool maxed;
        //            double angle;

        //            // Make sure we hit every grid cell
        //            step_size = _grid_res;

        //            //a = pose.pos;
        //            ax = x;
        //            ay = y;

        //            // Erase the local map if using Bayes Rule
        //            //if (mode == MapMode.BayesRule)
        //            //    ClearLocal();

        //            // Ray-trace the grid
        //            for (i = 0; i < num_ranges; i++)
        //            {
        //                r = (double)ranges[i] / _range_factor;
        //                // There is a fudge here due to a bug in MRDS that returns
        //                // zero instead of max range for a laser miss
        //                if (r >= _range_max || r == 0)
        //                {
        //                    r = _range_max;
        //                    maxed = true;
        //                }
        //                else
        //                    maxed = false;

        //                // Compute range end-point
        //                // NOTE: Scans are backwards, i.e. right to left
        //                angle = -_range_start + i * _range_step;
        //                // Now adjust for the robot's orientation
        //                angle += theta;
        //                // Convert to Radians
        //                angle = angle * Math.PI / 180;
        //                // Calculate the actual coords of the laser ending point
        //                // Note that r is relative to the robot
        //                bx = r * Math.Cos(angle) + ax;
        //                by = r * Math.Sin(angle) + ay;

        //                // Compute ray line parameters : Difference between laser ending point and robot pose.
        //                dx = bx - ax;
        //                dy = by - ay;
        //                dr = Math.Sqrt(dx * dx + dy * dy);

        //                //Discretize the ray line according to grid size, which is allocated in step_size
        //                step_count = (int)Math.Floor(dr / step_size) + 2;
        //                //Set starting point.
        //                dx /= (step_count - 1);
        //                dy /= (step_count - 1);

        //                // Just to keep the compiler happy ...
        //                occ = (int)MapValues.UNKNOWN;

        //                // Walk the ray line and update the grid
        //                for (j = 0; j < step_count; j++)
        //                {
        //                    //Next point in line according to grid resolution and the next step given by 'j'.
        //                    px = ax + dx * j;
        //                    py = ay + dy * j;

        //                    //Adjust the point into the grid values.
        //                    nx = MapGridX(px);
        //                    ny = MapGridY(py);

        //                    if (MapGridValid(nx, ny))
        //                    {
        //                        switch (mode)
        //                        {
        //                            case MapMode.Counting:
        //                            default:
        //                                occ = (int)_mapData[nx, ny] + 1;
        //                                if (occ > 255)
        //                                    occ = 255;
        //                                break;

        //                            case MapMode.Overwrite:
        //                                occ = 255;
        //                                break;

        //                            //case MapMode.BayesRule:
        //                            //    occ = (int)MapValues.VACANT;
        //                            //    break;
        //                        }

        //                        //if (mode == MapMode.BayesRule)
        //                        //    _localMapData[nx, ny] = (byte)occ;
        //                        //else
        //                        _mapData[nx, ny] = (byte)occ;
        //                    }
        //                }

        //                // Place an obstacle at the end of the ray if the scan
        //                // was not maxed out (a "miss" returns the max range value)
        //                if (!maxed)
        //                {
        //                    px = ax + dx * step_count;
        //                    py = ay + dy * step_count;

        //                    nx = MapGridX(px);
        //                    ny = MapGridY(py);

        //                    if (MapGridValid(nx, ny))
        //                    {
        //                        switch (mode)
        //                        {
        //                            case MapMode.Counting:
        //                            default:
        //                                occ = (int)_mapData[nx, ny] - 1;
        //                                if (occ < 0)
        //                                    occ = 0;
        //                                break;

        //                            case MapMode.Overwrite:
        //                                occ = 0;
        //                                break;

        //                            //case MapMode.BayesRule:
        //                            //    occ = (int)MapValues.OBSTACLE;
        //                            //    break;
        //                        }

        //                        //if (mode == MapMode.BayesRule)
        //                        //    _localMapData[nx, ny] = (byte)occ;
        //                        //else
        //                        _mapData[nx, ny] = (byte)occ;
        //                    }
        //                }
        //            }

        //            //if (mode == MapMode.BayesRule)
        //            //    UpdateMapBayes();

        //            return;
        //        }


        //        // Update the map using Bayes Rule
        //        //private void UpdateMapBayes()
        //        //{
        //        //    int lx, ly;			// Coords in local map
        //        //    int gx, gy;			// Coords in global map
        //        //    //int rx, ry;			// Robot x,y on the global image
        //        //    //int cx, cy;			// Robot x,y on the local map (Centre of map)
        //        //    int localWidth, localHeight;
        //        //    int globalWidth, globalHeight;
        //        //    float p, occ, occOld, occNew;

        //        //    // Maybe the dimensions will be different one day ...
        //        //    localWidth = _grid_sx;
        //        //    localHeight = _grid_sy;
        //        //    globalWidth = _grid_sx;
        //        //    globalHeight = _grid_sy;

        //        //    for (ly = 0; ly < localHeight; ly++)
        //        //    {
        //        //        // Not used now
        //        //        //gy = ry + (ly - cy);
        //        //        gy = ly;
        //        //        // Protection in case we run off the edges of the
        //        //        // global map, which can happen!
        //        //        if ((gy >= 0) && (gy < globalHeight))
        //        //        {
        //        //            for (lx = 0; lx < localWidth; lx++)
        //        //            {
        //        //                // Only update if the new probability is NOT
        //        //                // equal to UNKNOWN, i.e. we have some new knowledge
        //        //                // about this cell
        //        //                if (_localMapData[lx, ly] != (byte)MapValues.UNKNOWN)
        //        //                {
        //        //                    // occNew is the new probability of NOT being occupied
        //        //                    // (This is because the map is inverted as far as
        //        //                    // probability is concerned in order to match the
        //        //                    // standard convention for colours)
        //        //                    occNew = (float)_localMapData[lx, ly];
        //        //                    // We have to check for the extreme values to
        //        //                    // avoid problems in the calculations later
        //        //                    if (occNew == 0.0f)
        //        //                        occNew = 1.0f;
        //        //                    if (occNew > 254.0f)
        //        //                        occNew = 254.0f;
        //        //                    occNew = occNew / 255.0f;

        //        //                    // Not used now
        //        //                    //gx = rx + (lx - cx);
        //        //                    gx = lx;
        //        //                    // Insurance against stepping off the map ...
        //        //                    if ((gx >= 0) && (gx < globalWidth))
        //        //                    {
        //        //                        // Calculate the current (old) probability
        //        //                        occOld = (float)_mapData[gx, gy] / 255.0f;

        //        //                        p = occOld * occNew / ((1.0f - occOld) * (1.0f - occNew));
        //        //                        occ = 255.0f * (p / (1.0f + p));
        //        //                        // Force it back into the required range of 1-254
        //        //                        if (occ < 1.0f)
        //        //                            occ = 1.0f;
        //        //                        if (occ > 254.0f)
        //        //                            occ = 254.0f;

        //        //                        // Save the new probability
        //        //                        _mapData[gx, gy] = (byte)occ;
        //        //                    }
        //        //                }
        //        //            }
        //        //        }
        //        //    }
        //        //    return;
        //        //}


        //        /// <summary>
        //        /// MapToImage - Convert the map to an image
        //        /// </summary>
        //        /// <remarks>
        //        /// Creates a bitmap from the map array
        //        /// NOTE: The bitmap will be 24-bit RGB which is the most common format.
        //        /// The bitmap will be greyscale, as is usual for an occpancy grid,
        //        /// with Black = Occupied, White = Free.
        //        /// However, coloured information can be written into it by the
        //        /// caller, e.g. a path for the robot, after the bitmap is returned.
        //        /// </remarks>
        //        /// <returns>Bitmap containing the map (24-bit RGB)</returns>
        //        public Bitmap MapToImage()
        //        {
        //            byte[] data = new byte[_grid_size * 3];
        //            int i, j, k;
        //            // Bitmaps are flipped in the Y direction, so process the
        //            // scanlines backwards
        //            k = 0;
        //            for (j = _grid_sy - 1; j >= 0; j--)
        //            {
        //                for (i = 0; i < _grid_sx; i++)
        //                {
        //                    // Insert the grid cell value for R, G and B
        //                    // This will make it greyscale
        //                    data[k++] = _mapData[i, j];    //Blue
        //                    data[k++] = _mapData[i, j];    //Green
        //                    data[k++] = _mapData[i, j];    //Red
        //                    #region Code For Displaying Victims, Threats and Kins in Danger. (Commented for now)
        //                    //if (_mapData[i, j] != 107 && _mapData[i, j] != 116 && _mapData[i, j] != 118)
        //                    //{
        //                    //    data[k++] = _mapData[i, j];    //Blue
        //                    //    data[k++] = _mapData[i, j];    //Green
        //                    //    data[k++] = _mapData[i, j];    //Red
        //                    //}
        //                    //else if (_mapData[i, j] == 107) //Kins are orange
        //                    //{
        //                    //    data[k++] = 0;    //Blue
        //                    //    data[k++] = 128;    //Green
        //                    //    data[k++] = 255;    //Red
        //                    //}
        //                    //else if (_mapData[i, j] == 118) //Victims are red
        //                    //{
        //                    //    data[k++] = 0;    //Blue
        //                    //    data[k++] = 0;    //Green
        //                    //    data[k++] = 255;    //Red
        //                    //}
        //                    //else if (_mapData[i, j] == 116) //Threats are yellow
        //                    //{
        //                    //    data[k++] = 0;    //Blue
        //                    //    data[k++] = 255;    //Green
        //                    //    data[k++] = 255;    //Red
        //                    //}
        //                    #endregion
        //                }
        //            }
        //            return (MakeBitmap(_grid_sx, _grid_sy, data));
        //        }


        //        /// <summary>
        //        /// MakeBitmap - Makes a Bitmap from raw data
        //        /// </summary>
        //        /// <param name="width">Bitmap Width</param>
        //        /// <param name="height">Bitmap Height</param>
        //        /// <param name="imageData">Raw image data in RGB format
        //        /// (Bitmaps are BGR with image flipped in Y direction)</param>
        //        /// <returns>Bitmap image (24-bit RGB)</returns>
        //        public Bitmap MakeBitmap(int width, int height, byte[] imageData)
        //        {
        //            // NOTE: This code implicitly assumes that the width is a multiple
        //            // of four bytes because Bitmaps have to be longword aligned.
        //            // We really should look at bmp.Stride to see if there is any padding.
        //            // However, the width and height come from the webcam and most cameras
        //            // have resolutions that are multiples of four.

        //            Bitmap bmp = new Bitmap(width, height, PixelFormat.Format24bppRgb);

        //            BitmapData data = bmp.LockBits(
        //                new Rectangle(0, 0, bmp.Width, bmp.Height),
        //                ImageLockMode.WriteOnly,
        //                PixelFormat.Format24bppRgb
        //            );

        //            Marshal.Copy(imageData, 0, data.Scan0, imageData.Length);

        //            bmp.UnlockBits(data);

        //            //Adjusts to the Z Axis being the vertical.
        //            bmp.RotateFlip(RotateFlipType.Rotate180FlipNone);
        //            return bmp;
        //        }


        //        // Draw an occupancy grid style map using LRF data
        //        // NOTE: This uses basic GDI calls. It was used for
        //        // proof-of-concept, but is not used anymore.
        //        /// <summary>
        //        /// DrawMap - Draws an occupancy grid from LRF data
        //        /// </summary>
        //        /// <param name="bmp">Bitmap to draw into</param>
        //        /// <param name="X">X-Position in meters</param>
        //        /// <param name="Y">Y-Position in meters</param>
        //        /// <param name="Theta">Orientation in degrees</param>
        //        /// <param name="AngularRange">Field of View of the LRF</param>
        //        /// <param name="DistanceMeasurements">Array of range scans in millimeters</param>
        //        //public void DrawMap(Bitmap bmp, float X, float Y, float Theta,
        //        //                int AngularRange, int[] DistanceMeasurements)
        //        //{
        //        //    double angle;
        //        //    int range;
        //        //    double rx, ry;
        //        //    int startX, startY;
        //        //    int endX, endY;

        //        //    // Avoid nasty surprises
        //        //    //if (_mapImage == null || g == null)
        //        //    //    return;
        //        //    //Bitmap _mapImage = new Bitmap(_grid_sx, _grid_sy, PixelFormat.Format24bppRgb);
        //        //    g = Graphics.FromImage(bmp);
        //        //    //g.Clear(Color.LightGray);

        //        //    // Loop invariants
        //        //    double startAngle = AngularRange / 2.0;
        //        //    // AngularResolution is zero! This is a bug that I have
        //        //    // reported and it should be fixed in V1.5. In the meantime,
        //        //    // calculate the value.
        //        //    double angleIncrement = (double)AngularRange / (double)DistanceMeasurements.Length;
        //        //    //            Pen whitePen = new Pen(Color.White);
        //        //    //            Pen blackPen = new Pen(Color.Black, 2);
        //        //    //            blackPen.EndCap = LineCap.Round;

        //        //    // First draw the free space
        //        //    // Note that this code draws the free space using a series
        //        //    // of white "rays" from the LRF. The result is some small
        //        //    // "slivers" that are not filled in between the rays.
        //        //    // Some people fill in the whole area outlined by the
        //        //    // hit points on the assumption that if two rays are close
        //        //    // together then there is probably nothing in between.
        //        //    // Strictly speaking this is not correct because the LRF
        //        //    // tells you nothing about the spaces between rays.
        //        //    startX = (int)(X / _grid_res) + bmp.Width / 2;
        //        //    startY = bmp.Height / 2 - (int)(Y / _grid_res);
        //        //    for (int x = 0; x < DistanceMeasurements.Length; x++)
        //        //    {
        //        //        range = DistanceMeasurements[x];
        //        //        // NOTE: Scans are backwards, i.e. right to left
        //        //        angle = -startAngle + x * angleIncrement;
        //        //        // Now adjust for the robot's orientation
        //        //        angle += Theta;
        //        //        // Convert to Radians
        //        //        angle = angle * Math.PI / 180;

        //        //        // The Simulated LRF returns zero if there is no hit
        //        //        // This is not the way that a real LRF works
        //        //        if (range <= 0)
        //        //            range = (int)(_range_max * _range_factor);

        //        //        rx = range * Math.Cos(angle) / _range_factor;
        //        //        ry = range * Math.Sin(angle) / _range_factor;
        //        //        endX = startX + (int)(rx / _grid_res);
        //        //        endY = startY - (int)(ry / _grid_res);
        //        //        // NOTE: This code relies on the fact that the DrawLine
        //        //        // method will clip at the edges of the bitmap!
        //        //        g.DrawLine(Pens.White, startX, startY, endX, endY);
        //        //    }

        //        //    // Now draw the obstacles at the points of the laser hits.
        //        //    // NOTE: It might seem inefficient to do this as two loops,
        //        //    // but this way the obstacles can overwrite the free space
        //        //    // in the case of roundoff errors.
        //        //    for (int x = 0; x < DistanceMeasurements.Length; x++)
        //        //    {
        //        //        range = DistanceMeasurements[x];
        //        //        // NOTE: Scans are backwards, i.e. right to left
        //        //        angle = -startAngle + x * angleIncrement;
        //        //        // Now adjust for the robot's orientation
        //        //        angle += Theta;
        //        //        // Convert to Radians
        //        //        angle = angle * Math.PI / 180;
        //        //        // The Simulated LRF returns zero if there is no hit
        //        //        // This is not the way that a real LRF works
        //        //        if (range <= 0)
        //        //            range = (int)(_range_max * _range_factor);

        //        //        // Only draw the obstacle if the range is not maxed out
        //        //        // because this indicates a "miss"
        //        //        if (range < (int)(_range_max * _range_factor))
        //        //        {
        //        //            rx = range * Math.Cos(angle) / _range_factor;
        //        //            ry = range * Math.Sin(angle) / _range_factor;
        //        //            endX = startX + (int)(rx / _grid_res);
        //        //            endY = startY - (int)(ry / _grid_res);
        //        //            /*
        //        //            // We need a small dot at the end of the laser ray
        //        //            // However, drawing a point with DrawLine does not work!
        //        //            // So make it a little longer
        //        //            int ix2, iy2;
        //        //            ix2 = ix;
        //        //            if (ix2 > bmp.Width / 2)
        //        //                ix2 = ix - 1;
        //        //            if (ix2 < bmp.Width / 2)
        //        //                ix2 = ix + 1;
        //        //            iy2 = iy;
        //        //            if (iy2 < bmp.Height / 2)
        //        //                iy2 = iy + 1;
        //        //            g.DrawLine(blackPen, ix, iy, ix2, iy2);
        //        //            */

        //        //            // Put a single pixel at the end of the ray
        //        //            // This does not give a nice dark boundary like
        //        //            // drawing a short line, but it is more correct
        //        //            // because technically the laser only gives data
        //        //            // at discrete angles and you can't say anything
        //        //            // about the area in between rays
        //        //            // NOTE: We have to check the bounds of the image
        //        //            // for SetPixel -- it does not clip
        //        //            if (endX >= 0 && endX < bmp.Width &&
        //        //                endY >= 0 && endY < bmp.Height)
        //        //                bmp.SetPixel(endX, endY, Color.Black);
        //        //        }
        //        //    }
        //        //}

        //    }
        //}

        #endregion
    }
}
